#include "stm32f4xx.h"
#include "Inertial.h"
#include "LineTracker.h"
#include "bluetooth.h"
#include "diagnostic.h"
#include "drive.h"
#include "steering.h"
#include "drive_control.h"
#include "steering_control.h"
#include "Rotary.h"
#include "SharpSensors.h"
#include "start.h"
#include "logic.h"
#include "logic_fast.h"

#define DIAG_ENABLE 0

int main(void)
{
  /* Ready flags */
  uint8_t data_done = 0;
  uint8_t logic_done = 0;
  uint8_t control_done = 0;
  uint8_t diag_done = 0;
  uint8_t mode = 0;

  Start_Init();
  /* mode = 1 if fast, mode = 0 if slow*/
  mode = GPIO_ReadInputDataBit(START_PORT, START_PIN);
  //while(!GPIO_ReadInputDataBit(START_PORT, START_PIN));

/* Initialize periphs */
  //Init_Inertial();
  LT_Init();
  SS_Init();
//#if DIAG_ENABLE
  Init_Bluetooth();
//#else
  // Init_BluetoothNotUsed();
//#endif
  InitDrivePeriphs();
  InitSteerPeriphs();
  InitRotary();

/* Working loop */
  while (1)
  {
	  /* Gathering data*/
	  if((TIM8->CNT > 0U) && (TIM8->CNT < 15000U) && (data_done != 1))
	  {
		  GetLTValues();
		  SS_GetValues();
		 // Inertial_Get();
		  speed = GetSpeed();
		  data_done = 1;

	  }
	  /* Logic and control */
	  if((TIM8->CNT > 15000U) && (TIM8->CNT <20000U) && (control_done != 1))
	  {

		  if(mode == 0)
		  {
			  Logic();
		  } else
		  {
			  LogicFast();
		  }
		  control_done = 1;

	  }

	  /* Diagnostic and misc. */
	  if((TIM8->CNT > 20000U) && (TIM8->CNT <25000U) && (diag_done != 1))
	  {
		  SendDiagData();
		  diag_done = 1;

	  }
	  /* Reset cycle ready flags */
	  if(TIM8->CNT > 38500U)
	  {
		  data_done = 0;
		  control_done = 0;
		  diag_done = 0;
	  }
  }
}
